#include "my_comm.h"
#include "leg_config.h"
#include "ros/ros.h"

#ifdef PLATFORM_RASPI_PI

#include <wiringPi.h>
#include <wiringSerial.h>

int fd;//串口文件描述符

#endif

comm_t comm = {
	.step = 0,
	.cnt  = 0,
};

int comm_init(void)
{
	#ifdef PLATFORM_RASPI_PI
	//初始化外设库
	if (wiringPiSetup() == -1) {
		ROS_INFO("init wiringPi error\n");
		return -1;
	}

	//打开串口
	if ((fd = serialOpen("/dev/ttyS0", 115200)) == -1) {
		ROS_INFO("open serial fail!\n");
		exit(-1);
	}

	ROS_INFO("init wiringPi succeed!\n");
	#endif
}

void comm_exit(void)
{
	#ifdef PLATFORM_RASPI_PI
	serialClose(fd);//关闭串口
	#endif

	ROS_INFO("usart exit succeed!\n");
}

/**
 * @brief  数据发送
 * @param  
 * @retval 无
 */
void comm_tx(uint8_t fun, uint8_t *data, uint8_t datalen)
{
	uint8_t SendBuff[60];
    uint8_t len = 0;

    /* 帧头 */
    SendBuff[len++] = 0xFD;
    SendBuff[len++] = 0xDF;

    /* 总长度 先占位 */
    SendBuff[len++] = 0;

    /* 功能帧 */
    SendBuff[len++] = fun;

    /* 数据 */
    for (uint16_t max = (datalen + len), now = len; len < max; len++) {
        SendBuff[len] = data[len - now];
    }

    /* 获取实际长度 */
    SendBuff[2] = len;

    /* 校验位 */
    SendBuff[len] = 0;
    for (uint16_t i = 0; i < len; i++) {
        SendBuff[len] += SendBuff[i];
    }
		
	for (uint16_t i=0; i < len+1; i++) {
		serialPutchar(fd, SendBuff[i]);
	}
	
	// serialPutchar(fd, 'c');
	// ROS_INFO("usart tx");
	// HAL_UART_Transmit(&huart_comm, SendBuff, len+1, 0xff);
}

/**
 * @brief  蓝牙发送8位数据
 * @param  
 * @retval 无
 */
void comm_tx_u8(uint8_t fun, uint8_t data)
{
	comm_tx(fun, &data, 1);
}

/**
 * @brief  蓝牙发送16位数据
 * @param  
 * @retval 无
 */
void comm_tx_u16(uint8_t fun, uint16_t data)
{
	uint8_t u8_data[2] = {
		(uint8_t)(data>>8),
		(uint8_t)(data&0xff),
	};
	
	comm_tx(fun, u8_data, 2);
}

/**
 * @brief  根据上位机命令做出对应响应
 * @param  void
 * @retval void
 */
void comm_switch(void)
{
	uint8_t *rxdata = &comm.buff[4];
	
	switch(comm.buff[3])
	{
		case motor_angle:

			break;
	}
}
		
/**
* @brief  上位机指令解析 
 * @param  void
 * @retval void
 */
void comm_parsing(void)
{
	uint8_t ack = 0;
	
	for (int i=0; i < comm.rxlen; i++) {
		ack += comm.buff[i];
	}
	
	if (ack == comm.buff[comm.rxlen]) {
		comm_switch();
	}	
}

/**
 * @brief  蓝牙接收服务函数
 * @param  
 * @retval 无
 */
void comm_uart_handler(uint8_t data)
{
	switch(comm.step) {
		case 0:
			comm.cnt = 0;
			comm.buff[comm.cnt++] = data;
			if (data == 0xFD) {
				comm.step = 1;
			}
			break;
		case 1:
			comm.buff[comm.cnt++] = data;
			if (data == 0xDF) {
				comm.step = 2;
			} else {
				comm.step = 0;
			}
			break;
		case 2:
			comm.buff[comm.cnt++] = data;
		
			if (comm.cnt - 1 == 2) {
				comm.rxlen = data;
			}
			
			if (comm.cnt > comm.rxlen) {
				comm.step = 3;
				comm.cnt = 0;
			}
			
			break;
			
		default :
		{};
	}
	
	if (comm.step == 3) {
		comm.step = 0;
		comm_parsing();
	}
}

